#include "defaults.h"
#include "curve.h"
#include "utils.h"
#include "math/bsplines.h"
#include "math/numerics.h"
#include "timing/time.h"
#include "wrapper/superluwrapper.h"
#include <boost/numeric/ublas/operation_sparse.hpp>

using namespace std;

DerivativesDim1::DerivativesDim1(size_t d) : D(boost::extents[d+1]), Dw(boost::extents[d+1]),d(d) {} 

Curve::Curve(const vector<Vec4n>& Pw, size_t p, const Knotvector& U) {
	this->Pw = Pw;
	this->p = p;
	this->U = U;
	this->n = Pw.size() - 1;
}

Curve::Curve(const vector<Vec3n>& Pw3, size_t p, const Knotvector& U) {
	for(size_t i = 0; i < Pw3.size(); i++)
		Pw.push_back(math::V4n(Pw3[i], 1.0));
	this->p = p;
	this->U = U;
	this->n = Pw.size() - 1;
}

Curve::Curve(const Curve& curve) {
	Pw = curve.Pw;
	p = curve.p;
	U = curve.U;
	n = Pw.size() - 1;
}

void Curve::Save(const string& filename) {
	ofstream file;
	file.open(filename.c_str());
	if(!file) {
		cerr << "Could not open file \"" << filename << "\"." << endl;
	} else {
		file << setprecision(32);
		file << "curve" << endl;
		file << p << endl;
		file << n + 1 << endl;
		for(size_t i = 0; i < Pw.size(); i++) {
			file << Pw[i][0] << " " << Pw[i][1] << " " << Pw[i][2] << " " << Pw[i][3] << endl;
		}
		file << endl;
		file << U.size() << endl;
		for(size_t i = 0; i < U.size() - 1; i++)
			file << U[i] << " ";
		file << U[U.size() - 1];
		file << endl;
		file.close();
	}
}

void Curve::Save(const string& filename, const string& attribute) {
	this->Save(filename);	
}

void Curve::Derivative(nfloat* param, Derivatives* pd) const {
	DerivativesDim1& SKL = static_cast<DerivativesDim1&>(*pd);

	typedef boost::multi_array<Vec3n, 1>::index index;
	boost::multi_array<Vec4n, 1>& Awders = SKL.Dw;

	size_t du = SKL.d;

	size_t span = math::spline::FindSpan<nfloat>(n, p, param[0], U);
	vector< vector<nfloat> > nders = math::spline::DersBasisFuns<nfloat>(span, param[0], p, du, U);

	for(size_t k = 0; k <= du; k++) {
		Awders[k] = 0.0;
		for(size_t j = 0; j <= p; j++) {
			Awders[k] = Awders[k] + nders[k][j] * Pw[span - p + j];
		}
	}

	boost::multi_array<Vec3n, 1>& CK = SKL.D;
	for(size_t k = 0; k <= du; k++) {
		Vec3n v = math::V3d(Awders[k][0], Awders[k][1], Awders[k][2]);
		for(size_t i = 1; i <= k; i++)
			v = v - Bin[k][i] * Awders[i][3] * CK[k - i];
		CK[k] = v * ((nfloat)1.0 / Awders[0][3]);
	}
}

Vec3n Curve::Evaluate(const boost::array<nfloat, 1>& param) const {
	const nfloat& u = param[0];
	size_t span = math::spline::FindSpan<nfloat>(n, p, u, U);
	vector<nfloat> N(p + 1);
	math::spline::BasisFuns<nfloat>(span, u, p, U, N);
	Vec4n Cw;
	for(size_t i = 0; i <= p; i++) {
		Cw = Cw + N[i] * Pw[span - p + i];
	}

	nfloat winv = 1.0 / Cw[3];
	Vec3n C = math::V3d(Cw[0] * winv, Cw[1] * winv, Cw[2] * winv);
	return C;
}

Vec3n Curve::Evaluate(nfloat t) const {
	boost::array<nfloat, 1> param = {t};
	return Evaluate(param);
}

nfloat CalculateTOL(const vector<Vec4n>& Pw, nfloat d) {
	nfloat wmin = numeric_limits<nfloat>::max();
	nfloat pmax = 0;
	for(size_t i = 0; i < Pw.size(); i++) {
		nfloat w = Pw[i][3];
		if(w < wmin)
			wmin = w;
		nfloat l = length(Pw[i]);
		if(l > pmax)
			pmax = l;
	}
	return (d * wmin) / (1.0 + pmax);
}

size_t Curve::RemoveCurveKnot(size_t n, size_t p, Knotvector& U, vector<Vec4n>& Pw, nfloat u,
			   size_t r, size_t s, size_t num, size_t& t) {
	size_t m = U.size();// n + p + 1;
	size_t ord = p + 1;
	size_t fout = (2 * r - s - p) / 2;
	size_t last = r - s;// 7 - 3
	size_t first = r - p;// 7 - 3;
	nfloat TOL = CalculateTOL(Pw, 1.0);
	vector<Vec4n> temp(2 * p + 1);
	for(t = 0; t < num; t++) {
		size_t off = first - 1;
		temp[0] = Pw[off];
		temp[last + 1 - off] = Pw[last + 1];
		size_t i = first;
		size_t j = last;
		size_t ii = 1;
		size_t jj = last - off;
		nfloat alfi;
		nfloat alfj;
		int remflag = 0;
		while(j - i > t) {
			alfi = (u - U[i]) / (U[i + ord + t] - U[i]);
			alfj = (u - U[j - t]) / (U[j + ord] - U[j - t]);
			temp[ii] = (Pw[i] - ((nfloat)1.0 - alfi) * temp [ii - 1]) / alfi;
			temp[jj] = (Pw[j] - alfj * temp[jj + 1]) / ((nfloat)1.0 - alfj);
			i++;
			ii++;
			j--;
			jj--;
		}
		if(j - i < t) {
			nfloat l = length(temp[ii - 1] - temp[jj + 1]);
			if(l <= TOL)
				remflag = 1;
		} else {
			alfi = (u - U[i]) / (U[i + ord + t] - U[i]);
			Vec4n P = alfi * temp[ii + t + 1] + ((nfloat)1.0 - alfi) * temp[ii - 1];
			if(length(Pw[i] - P) <= TOL)
				remflag = 1;
		}
		if(remflag == 0) {
			break;
		} else {
			i = first;
			j = last;
			while(j - i > t) {
				Pw[i] = temp[i - off];
				Pw[j] = temp[j - off];
				i++;
				j--;
			}
		}
		first--;
		last++;
	}
	if(t == 0)
		return -1;

	for(size_t k = r + 1; k <= m; k++)
		U[k - t] = U[k];
	U.resize(U.size() - t);

	size_t j = fout;
	size_t i = j;
	for(size_t k = 1; k < t; k++)
		if((k % 2) == 1)
			i++;
		else
			j--;

	for(size_t k = i + 1; k <= n; k++) {
		Pw[j] = Pw[k];
		j++;
	}
	Pw.resize(Pw.size() - t);

	return 0;
}

bool Curve::Remove(nfloat u, size_t num) {
	if(num <= 0)
		return false;

	size_t s, t, r;
	size_t i = 0;

	for(size_t i = 0; i < U.size(); i++) {
		if(fabs(U[i] - u) < 0.000001) {
			r = i;
			break;
		}
	}

	if(i == U.size())
		return false;

	s = 1;
	for(size_t i = r + 1; i < U.size(); i++) {
		if(fabs(U[i] - u) < 0.000001) {
			s++;
		} else
			break;
	}
	r = r + s - 1;

	RemoveCurveKnot(n, p, U, Pw, u, r, s, num, t);
	n -= t;

	return true;
}

void Curve::RefineKnotVectCurve(size_t n, size_t p, const Knotvector& U, const vector<Vec4n>& Pw, const Knotvector& X, size_t r, Knotvector& Ubar, vector<Vec4n>& Qw) {
	size_t m = n + p + 1;
	size_t a = math::spline::FindSpan<nfloat>(n, p, X[0], U);
	size_t b = math::spline::FindSpan<nfloat>(n, p, X[r], U);
	b++;

	for(size_t j = 0; j <= a - p; j++) {
		Qw[j] = Pw[j];
	}
	for(size_t j = b - 1; j <= n; j++) {
		Qw[j + r + 1] = Pw[j];
	}

	for(size_t j = 0; j <= a; j++)
		Ubar[j] = U[j];
	for(size_t j = b + p; j <= m; j++)
		Ubar[j + r + 1] = U[j];

	size_t i = b + p - 1;
	size_t k = b + p + r;

	for(int j = (int)r; j >= 0; j--) {
		while(X[(size_t)j] <= U[i] && i > a) {
			Qw[k - p - 1] = Pw[i - p - 1];
			Ubar[k] = U[i];
			k--;
			i--;
		}
		Qw[k - p - 1] = Qw[k - p];
		for(size_t l = 1; l <= p; l++) {
			size_t ind = k - p + l;
			nfloat alfa = Ubar[k + l] - X[j];
			if(std::abs(alfa) < 0.000001)
				Qw[ind - 1] = Qw[ind];
			else {
				alfa /= (Ubar[k + l] - U[i - p + l]);
				Qw[ind - 1] = alfa * Qw[ind - 1] + ((nfloat)1.0 - alfa) * Qw[ind];
			}
		}
		Ubar[k] = X[j];
		k--;
	}
}

void Curve::Refine() {
	size_t r = U.size() - 2 * p - 2;
	vector<nfloat> X(r + 1);

	for(size_t i = p; i < U.size() - p - 1; i++)
		X[i - p] = 0.5 * (U[i + 1] - U[i]) + U[i];

	std::vector<Vec4n> Qw(n + r + 2);
	Knotvector Ubar(U.size() + r + 1);

	RefineKnotVectCurve(n, p, U, Pw, X, r, Ubar, Qw);
	Pw = Qw;
	U = Ubar;
	n = Pw.size() - 1;
}

void Curve::Refine(const vector<Knotvector>& X) {
	size_t r = X[0].size() - 1;
	std::vector<Vec4n> Qw(n + r + 2);
	Knotvector Ubar(U.size() + r + 1);

	RefineKnotVectCurve(n, p, U, Pw, X[0], r, Ubar, Qw);
	Pw = Qw;
	U = Ubar;
	n = Pw.size() - 1;
}

void Curve::Refine(nfloat t) {
	vector<Knotvector> X(1);
	X[0].push_back(t);
	Refine(X);
}

static nfloat AreaTriangle(const Vec3n& v0, const Vec3n& v1, const Vec3n& v2) {
	return 0.5 * length(cross(v1 - v0, v2 - v0));
}

static bool Collinear(const vector<Vec3n>& Q, size_t start, size_t end, nfloat E = 0.000001) {
	const Vec3n& Ps = Q[start];
	const Vec3n& Pe = Q[end];

	for(size_t i = start + 1; i < end; i++) {
		if(AreaTriangle(Ps, Pe, Q[i]) > E)
			return false;
	}
	return true;
}

nfloat Curve::Curvature(nfloat u) const {
	DerivativesDim1 SKL(2);
	nfloat p[1] = {u};
	Derivative(p, &SKL);
	const Vec3n& beta1 = SKL.D[1];
	const Vec3n& beta2 = SKL.D[2];
	return length(cross(beta1, beta2)) / std::pow(length(beta1), 3.0);
}

void Curve::binomialCoef(Matrix& Bin) const {
	int n, k;
	// Setup the first line
	Bin(0, 0) = 1.0;
	for(k = Bin.size2() - 1; k > 0; --k)
		Bin(0, k) = 0.0;

	// Setup the other lines
	for(n = 0; n < Bin.size1() - 1; n++) {
  		Bin(n + 1, 0) = 1.0;
  		for(k = 1; k < Bin.size2(); k++)
			if(n + 1 < k)
				Bin(n, k) = 0.0 ;
			else
				Bin(n + 1, k) = Bin(n, k) + Bin(n, k - 1);
	}
}

void Curve::DegreeElevate(int t) {
	if(t <= 0){
		return;
	}

	Curve c = *this;

	int i, j, k;
	int n = c.Size() - 1;
	int p = c.GetDegree();
	int m = n + p + 1;
	int ph = p + t;
	int ph2 = ph / 2;
	Matrix bezalfs = ZeroMatrix(p + t + 1, p + 1); // coefficients for degree elevating the Bezier segment
	
	vector<Vec4n> bpts(p + 1); // pth-degree Bezier control points of the current segment
	vector<Vec4n> ebpts(p + t + 1); // (p+t)th-degree Bezier control points of the  current segment
	vector<Vec4n> Nextbpts(p - 1); // leftmost control points of the next Bezier segment
	Vector alphas = ZeroVector(p - 1); // knot instertion alphas.

	// Compute the binomial coefficients
	Matrix Bin(ph + 1, ph2 + 1);
	binomialCoef(Bin);

	// Compute Bezier degree elevation coefficients
	nfloat inv, mpi;
	bezalfs(0, 0) = bezalfs(ph, p) = 1.0;
	for(i = 1; i <= ph2; i++) {
		inv= 1.0 / Bin(ph, i);
		mpi = std::min(p, i);
		for(int j = std::max(0, i - t); j <= mpi; j++) {
			bezalfs(i, j) = inv * Bin(p, j) * Bin(t, i - j) ;
		}
	}

	for(i = ph2 + 1; i < ph; i++) {
		mpi = std::min(p, i);
		for(j = std::max(0, i - t); j <= mpi; j++)
			bezalfs(i, j) = bezalfs(ph - i, p - j);
	}

	this->Pw.resize(c.Size() + c.Size() * t); // Allocate more control points than necessary
	this->p = ph;
	this->U.resize(Size() + this->p + 1);

	int mh = ph;
	int kind = ph + 1;
	nfloat ua = c.U[0];
	nfloat ub = 0.0;
	int r = -1; 
	int oldr;
	int a = p;
	int b = p + 1;
	int cind = 1;
	int rbz, lbz = 1; 
	int mul, save, s;
	nfloat alf;
	int first, last, kj;
	nfloat den, bet, gam, numer;

	Pw[0] = c.Pw[0];
	for(i = 0; i <= ph; i++) {
		U[i] = ua;
	}

	// Initialize the first Bezier segment

	for(i = 0; i <= p; i++)
		bpts[i] = c.Pw[i];

	while(b < m) { // Big loop thru knot vector
		i = b;
		while(b < m && c.U[b] >= c.U[b + 1]) // for some odd reasons... == doesn't work
			b++;
		mul = b - i + 1; 
		mh += mul + t;
		ub = c.U[b];
		oldr = r;
		r = p - mul;
		if(oldr > 0)
			lbz = (oldr + 2) / 2;
		else
			lbz = 1;
		if(r > 0) 
			rbz = ph - (r + 1) / 2;
		else
			rbz = ph;
		if(r > 0) { // Insert knot to get Bezier segment
			numer = ub - ua;
			for(k = p; k > mul; k--) {
				alphas(k - mul - 1) = numer / (c.U[a + k] - ua);
			}
			for(j = 1; j <= r; j++) {
				save = r - j; s = mul + j;
				for(k = p; k >= s; k--) {
					bpts[k] = alphas(k - s) * bpts[k] + (1.0 - alphas(k - s)) * bpts[k - 1];
				}
				Nextbpts[save] = bpts[p];
			}
		}
    
		for(i = lbz; i <= ph; i++) { // Degree elevate Bezier,  only the points lbz,...,ph are used
			ebpts[i] = 0.0;
			mpi = std::min(p, i);
			for(j = std::max(0, i - t); j <= mpi; j++)
				ebpts[i] += bezalfs(i, j) * bpts[j];
		}

		if(oldr > 1) { // Must remove knot u=c.U[a] oldr times
			// if(oldr>2) // Alphas on the right do not change
			//	alfj = (ua-U[kind-1])/(ub-U[kind-1]) ;
			first = kind - 2; last = kind;
			den = ub - ua;
			bet = (ub - U[kind - 1]) / den;
			for(int tr = 1; tr < oldr; tr++) { // Knot removal loop
				i = first; j = last;
				kj = j - kind + 1;
				while(j - i > tr) { // Loop and compute the new control points for one removal step
					if(i < cind) {
						alf = (ub - U[i]) / (ua - U[i]);
						Pw[i] = alf * Pw[i] + (1.0 - alf) * Pw[i - 1];
					}
					if(j >= lbz) {
						if(j - tr <= kind - ph + oldr) {
							gam = (ub - U[j - tr]) / den;
							ebpts[kj] = gam * ebpts[kj] + (1.0 - gam) * ebpts[kj + 1];
						}
						else {
							ebpts[kj] = bet * ebpts[kj] + (1.0 - bet) * ebpts[kj + 1];
						}
					}
					++i; --j; --kj;
				}
				--first; ++last;
			}
		}

		if(a != p) // load the knot u=c.U[a]
			for(i = 0; i < ph - oldr; i++) {				
				U[kind++] = ua;
			}
		for(j = lbz; j <= rbz; j++) { // load control points onto the curve
			Pw[cind++] = ebpts[j];
		}

		if(b < m) { // Set up for next pass thru loop
			for(j = 0; j < r; j++)
				bpts[j] = Nextbpts[j] ;
			for(j = r; j <= p; j++)
				bpts[j] = c.Pw[b - p + j];
			a = b; 
			b++;
			ua = ub;
		} else {
			for(i = 0; i <= ph; i++) {
				U[kind + i] = ub;
			}
		}
	}
	
	Pw.resize(mh - ph) ; // Resize to the proper number of control points
	this->n = Pw.size() - 1;
	
	Knotvector U2 = U;
	U.resize((mh - ph) + this->p + 1);
		
	for(size_t i = 0; i < U.size(); i++)
		U[i] = U2[i];
}

Matrix Curve::TBN(const nfloat t) const {
	DerivativesDim1 SKL(2);
	nfloat param[1];
	param[0] = t;
	Derivative(param, &SKL);

	Vec3n tangent = SKL.D[1];
	normalize(tangent);

	Vec3n binormal = cross(SKL.D[1], SKL.D[2]);
	normalize(binormal);

	Vec3n normal = cross(binormal, tangent);
	normalize(normal);

	Matrix TBN(3, 3);
	TBN(0, 0) = tangent[0]; TBN(0, 1) = tangent[1]; TBN(0, 2) = tangent[2]; 
	TBN(1, 0) = binormal[0]; TBN(1, 1) = binormal[1]; TBN(1, 2) = binormal[2];
	TBN(2, 0) = normal[0]; TBN(2, 1) = normal[1]; TBN(2, 2) = normal[2]; 

	return TBN;
}
